// youminkim@20101110
// TODO: initial value
// TODO: periodic reset message

int WHITE_UPPER = 150;
int THRESHOLD_BW = 150;

int a[16];
int v[4][4];
int tag[4];
int preTag[4];

int gi = 0;
int seq[5];
boolean isMarkerOn[4];
boolean preIsMarkerOn[4];
boolean isStable = false;

void setup() {
  Serial.begin(9600); 

  pinMode(2, OUTPUT); 
  pinMode(3, OUTPUT); 
  pinMode(4, OUTPUT); 
  pinMode(5, OUTPUT);

  // init
  for (int i = 0 ; i < 4; i++){
    isMarkerOn[i] = false;
    preIsMarkerOn[i] = false;
    preTag[i] = 30; 
    seq[i] = i;
  }
}

void loop() {

  // get values
  getAnalogValues();

  // make readable arrangement
  arrangeData();


////////// test code
/*
  for (int i = 0 ; i < 4; i++){
    for (int j = 0; j < 4; j++){
      Serial.print(v[i][j]);
      Serial.print("\t");
    }
  }
  Serial.println("");
  
  delay(100);
  return;
*/
////////////


  // is stable?
  // 1) calculate avg and save to the array 
  // 2) if all sequences of the array are same, then it is stable
  float sum2 = 0;
  for (int i = 0; i < 4; i++){
    for (int j = 0; j < 4; j++){
      sum2 += v[i][j];   
    }
  }
  sum2 = sum2/ 16;
  seq[gi%5] = int(sum2);
  gi++;
  
  float avg = 0;
  for (int i = 0; i < 5; i++){
    avg += seq[i];
  }
  avg = avg/5;
  
  float diff = 0;
  for (int i = 0; i < 5; i++){
    diff += (avg-seq[i])*(avg-seq[i]);
  }
  
  //Serial.println(sqrt(diff)/5);
  
  //delay(50);
  //return;
  
  /*
  if (seq[0] == seq[1] && seq[1] == seq[2] && seq[2] == seq[3] && seq[3] == seq[4]){
    isStable = true; 
  } 
  else {
    isStable = false; 
  }*/
  
  if (diff < 1.0){
    isStable = true;
  } else {
    isStable = false; 
  }

  // if unstable, then quit
  /*
  if (!isStable) {
    delay(500);
    return;
  }*/

  // check marker on by getting the white tag. at least one white tag, then marker is on
  for (int i = 0; i < 4; i++){
    isMarkerOn[i] = false;
    for (int j = 0; j < 4; j++){
      if (v[i][j] < WHITE_UPPER){
        isMarkerOn[i] = true; 
      }
    }
  }

  // determine marker is white or black
  for (int i = 0 ; i < 4; i++){
    if (isMarkerOn[i]){
      for (int j = 0; j < 4; j++){

        // determine the tag is black or not
        if (v[i][j] > THRESHOLD_BW){
          v[i][j] = 0; // black   
        } 
        else {
          v[i][j] = 1; // white
        }
      }
    } 
    else {
      continue;
    }
  }

  // assign tag
  for (int i = 0; i < 4; i++){
    int sum = 0;
    int b = 1;
    for (int j = 0; j < 4; j++){
      sum += v[i][j]*b;
      b *= 2;
    } 
    tag[i] = sum;
  }



  // command
  for (int i = 0; i < 4; i++){
    if (isMarkerOn[i] && !preIsMarkerOn[i]){
      noteOn(9,tag[i]+i*16,127);
      //Serial.print(tag[i]+i*16);
      //Serial.print("\t");
      //Serial.println("marker on");
      preTag[i] = tag[i];
      preIsMarkerOn[i] = true;
      delay(500);
    } 
    else if (!isMarkerOn[i] && preIsMarkerOn[i]){
      noteOn(10,preTag[i] + i*16,127);
      //Serial.print(preTag[i] + i*16);
      //Serial.print("\t");
      //Serial.println("marker off");
      preTag[i] = -1;
      preIsMarkerOn[i] = false;
      delay(500);
    }
  }
  delay(50);
}

////////////// s2midi


// Send a MIDI note-on message
void noteOn(byte channel, byte note, byte velocity) {
  midiMsg(channel+0x90, note, velocity);
}

// Send a MIDI control change
void controlChange(byte channel, byte controller, byte value) {
  midiMsg(channel+0xB0, controller, value);
}

// Send a general MIDI message
void midiMsg(byte cmd, byte data1, byte data2) {
  Serial.print(cmd, BYTE);
  Serial.print(data1, BYTE);
  Serial.print(data2, BYTE);
}

void getAnalogValues(){
  // read the analog input into a variable:
  digitalWrite(2,LOW);
  digitalWrite(3,LOW);
  a[0] = analogRead(0);
  a[1] = analogRead(1);

  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  a[2] = analogRead(2);
  a[3] = analogRead(3);

  delay(1);

  digitalWrite(2,HIGH);
  digitalWrite(3,LOW);
  a[4] = analogRead(0);
  a[5] = analogRead(1);

  digitalWrite(4,HIGH);
  digitalWrite(5,LOW);
  a[6] = analogRead(2);
  a[7] = analogRead(3);

  delay(1);

  digitalWrite(2,LOW);
  digitalWrite(3,HIGH);
  a[8] = analogRead(0);
  a[9] = analogRead(1);

  digitalWrite(4,LOW);
  digitalWrite(5,HIGH);
  a[10] = analogRead(2);
  a[11] = analogRead(3);

  delay(1);

  digitalWrite(2,HIGH);
  digitalWrite(3,HIGH);
  a[12] = analogRead(0);
  a[13] = analogRead(1);

  digitalWrite(4,HIGH);
  digitalWrite(5,HIGH);
  a[14] = analogRead(2);
  a[15] = analogRead(3);

  delay(1); 
}

void arrangeData(){
  v[0][0] = a[11];
  v[0][1] = a[7];
  v[0][2] = a[4];
  v[0][3] = a[8];

  v[1][0] = a[3];
  v[1][1] = a[15];
  v[1][2] = a[12];
  v[1][3] = a[0];

  v[2][0] = a[2];
  v[2][1] = a[10];
  v[2][2] = a[5];
  v[2][3] = a[1];

  v[3][0] = a[14];
  v[3][1] = a[6];
  v[3][2] = a[13];
  v[3][3] = a[9]; 
}

void checkAvg(){
  for (int i = 0 ; i < 4; i++){
    float sum = 0;
    for (int j = 0; j < 4; j++){
      sum+= v[j][i]; 
    }
    sum = sum/4;
    Serial.print(sum);
    Serial.print("\t");
  }
  Serial.println(""); 
}














